i1 = imread('5.jpg');
i2 = imread('6.jpg');
figure
imshowpair(i1, i2, 'montage');
Warning: Image is too big to fit on screen; displaying at 17%
title('Original Images');
I1 = undistortImage(i1,cameraParams);
I2 = undistortImage(i2,cameraParams);
figure
imshowpair(I1, I2, 'montage');
Warning: Image is too big to fit on screen; displaying at 17%
title('Undistorted Images');
% Detect feature points
imagePoints1 = detectSURFFeatures(rgb2gray(I1));
% Visualize detected points
figure
imshow(I1, 'InitialMagnification', 50);
Warning: Image is too big to fit on screen; displaying at 17%
title('150 Strongest Corners from the First Image');
hold on
plot(selectStrongest(imagePoints1, 150));
% Create the point tracker
tracker = vision.PointTracker('MaxBidirectionalError', 1, 'NumPyramidLevels', 5);
% Initialize the point tracker
imagePoints1 = imagePoints1.Location;
initialize(tracker, imagePoints1, I1);
% Track the points
[imagePoints2, validIdx] = step(tracker, I2);
matchedPoints1 = imagePoints1(validIdx, :);
matchedPoints2 = imagePoints2(validIdx, :);
% Visualize correspondences
figure
showMatchedFeatures(I1, I2, matchedPoints1, matchedPoints2);
Warning: Image is too big to fit on screen; displaying at 17%
title('Tracked Features');
% Estimate the fundamental matrix
[E, epipolarInliers] = estimateEssentialMatrix(...
matchedPoints1, matchedPoints2, cameraParams, 'Confidence', 99.99);
% Find epipolar inliers
inlierPoints1 = matchedPoints1(epipolarInliers, :);
inlierPoints2 = matchedPoints2(epipolarInliers, :);
% Display inlier matches
figure
showMatchedFeatures(I1, I2, inlierPoints1, inlierPoints2);
Warning: Image is too big to fit on screen; displaying at 17%
title('Epipolar Inliers');
% Compute the Camera Pose
[orient, loc] = relativeCameraPose(E, cameraParams, inlierPoints1, inlierPoints2);
% Detect dense feature points. Use an ROI to exclude points close to the
% image edges.
roi = [30, 30, size(I1, 2) - 30, size(I1, 1) - 30];
imagePoints1 = detectMinEigenFeatures(rgb2gray(I1), 'ROI', roi, ...
'MinQuality', 0.001);
% Create the point tracker
tracker = vision.PointTracker('MaxBidirectionalError', 1, 'NumPyramidLevels', 5);
% Initialize the point tracker
imagePoints1 = imagePoints1.Location;
initialize(tracker, imagePoints1, I1);
% Track the points
[imagePoints2, validIdx] = step(tracker, I2);
matchedPoints1 = imagePoints1(validIdx, :);
matchedPoints2 = imagePoints2(validIdx, :);
% Compute the camera matrices for each position of the camera
% The first camera is at the origin looking along the Z-axis. Thus, its
% rotation matrix is identity, and its translation vector is 0.
camMatrix1 = cameraMatrix(cameraParams, eye(3), [0 0 0]);
% Compute extrinsics of the second camera
[R, t] = cameraPoseToExtrinsics(orient, loc);
camMatrix2 = cameraMatrix(cameraParams, R, t);
% Compute the 3-D points
points3D = triangulate(matchedPoints1, matchedPoints2, camMatrix1, camMatrix2);
% Get the color of each reconstructed point
numPixels = size(I1, 1) * size(I1, 2);
allColors = reshape(I1, [numPixels, 3]);
colorIdx = sub2ind([size(I1, 1), size(I1, 2)], round(matchedPoints1(:,2)), ...
round(matchedPoints1(:, 1)));
color = allColors(colorIdx, :);
% Create the point cloud
ptCloud = pointCloud(points3D, 'Color', color);
% Visualize the camera locations and orientations
cameraSize = 0.3;
figure
plotCamera('Size', cameraSize, 'Color', 'r', 'Label', '1', 'Opacity', 0);
hold on
grid on
plotCamera('Location', loc, 'Orientation', orient, 'Size', cameraSize, ...
'Color', 'b', 'Label', '2', 'Opacity', 0);
% Visualize the point cloud
pcshow(ptCloud, 'VerticalAxis', 'y', 'VerticalAxisDir', 'down', ...
'MarkerSize', 45);
% Rotate and zoom the plot
camorbit(0, -30);
camzoom(1.5);
% Label the axes
xlabel('x-axis');
ylabel('y-axis');
zlabel('z-axis')
title('Up to Scale Reconstruction of the Scene');